#include "app.h"

// #######################################################################################################
// KEYBOARD
// #######################################################################################################
void servo_Init(void) {
	servus_Init();
	uint x;	for (x=0;x<SERVO_N_MODULE;x++) {
		control.servo[x].timeActive = 0;// tempo infinito que o servo ficará ativo
		control.servo[x].countIdle = 0;
		control.servo[x].pos = 1500;// TODO: ler valores eeprom
		control.servo[x].posNew = control.servo[x].pos;
		control.servo[x].min = 100; // TODO: ler valores eeprom
		control.servo[x].max = 3000; // TODO: ler valores eeprom
		control.servo[x].enabled = 1; // TODO: ler valores eeprom
	}
}

// Habilita ou não um determinado servo.
int servo_Enable(uint servo, uint en) {
	if ( (servo == 0) || (servo > SERVO_N_MODULE) ) return errDEVICE_NOT_FOUND;
	servus_Enable(servo, en);

	return pdPASS;
}


// Ajusta o posicionamento do servo desejado
//	servo: o servo alvo para posicionamento, valor de 1 a 6
//  us: largura do pulso em microsegundos de controle de posição do servos. Valor entre 100 a 3000uS
int servo_SetPos(uint servo, uint us) {
	if ( (servo==0) || (servo>SERVO_N_MODULE) ) return errDEVICE_NOT_FOUND;
	if ( (us<100) || (us>3000)) return errDEVICE_VALUE_INVALID;

	control.servo[servo-1].posNew = us;
	control.servo[servo-1].countIdle = 0;
	control.servo[servo-1].timeActive = timeActive*1000; // Multiplicar por 1000 porque o tempo de controle é em 1ms
	control.servo[servo-1].pos = 0; 	// para forçar a ir para posição nova, caso a última poisção registrada é a mesma para ir.
											// pode acontecer que o servo seja reposicionado manualmemente quanto saida pwm desligada
	control.servo[servo-1].enabled = 1;
	servus_Enable(servo, 1); // Liga o servo

	return pdPASS;
}


void servo_Tick(void) {
	uint x;	for (x=0;x<SERVO_N_MODULE;x++) { // fazer para todos os servos
		if (control.servo[x].pos != control.servo[x].posNew) { // se a posição for diferente vamos atualizar o servo
			control.servo[x].pos = control.servo[x].posNew;
			servus_SetPos(x+1, control.servo[x].pos); // atualizar a posição do servo
			control.servo[x].countIdle = 0; // sinaliza que o servo está se posicionando
		} else control.servo[x].countIdle++;

		// Desliga o servo caso foi especificado o tempo para manter ligado
		if ( (control.servo[x].timeActive > 0) && (control.servo[x].countIdle > control.servo[x].timeActive) )
 			servus_Enable(x+1, 0); // desliga o servo
	}
}


// #######################################################################################################
// KEYBOARD
// #######################################################################################################
void onKeyDown(char key) {
	lprintf("onKeyDown key [0x%x]"CMD_TERMINATOR, key);
	if (key == SW0) beep_Play(4);
	else if (key == SW1) beep_Play(5);
	else if (key == SW2) beep_Play(6);
}

void onKeyPress(char key __attribute__ ((unused)) ) {
	lprintf("onKeyPress key [0x%x] fastInc[%d] "CMD_TERMINATOR, key, gkey_FastInc);
}

void onKeyUp(char key __attribute__ ((unused)) ) {
	lprintf("onKeyUp key [0x%x] fastInc[%d] "CMD_TERMINATOR, key, gkey_FastInc);
}

// #######################################################################################################
// RELAYS
// #######################################################################################################
void relay_Init(void) {
	relay1_Init();
	relay2_Init();

	control.relay1.st = 0;
	control.relay1.timeout = 0;

	control.relay2.st = 0;
	control.relay2.timeout = 0;
}

// Liga ou desliga o rele
//	parametro rly: sinaliza qual o rele vamos acessar. valor 0 ou 1
//	parametro st: pdON liga o rele e pdOFF desliga o rele;
//	parametro timeout: O tempo em segundos que o rele vai ficar no estado atual e voltar para o estado inverso
//		Se colocar valor 0, permanece no estado atual até o próximo comando
int relay_Set(int rly, int st, uint timeout) {
	pRelay rl;

	if ( (rly < 0) || (rly > 1) ) return errDEVICE_NOT_FOUND;
	if (rly == 0) rl = &control.relay1; else rl = &control.relay2;

	rl->timeout = timeout*1000;
	rl->st = st;

	return pdPASS;
}

void relay_Tick(void) {
	if (control.relay1.st) relay1_On(); else relay1_Off();
	if (control.relay2.st) relay2_On(); else relay2_Off();
	if (control.relay1.timeout > 0) control.relay1.timeout--;
	if (control.relay2.timeout > 0) control.relay2.timeout--;
	//colocar depois do acionamento para que seja certinho a contagem dos tempos
	if (control.relay1.timeout == 1) control.relay1.st ^= 1;
	if (control.relay2.timeout == 1) control.relay2.st ^= 1;
}

// #######################################################################################################
// APPLICATION
// #######################################################################################################
void app_Init(void) {
	control.ticks = 0;
	control.key = 0;

	led1_Init();
	led2_Init();
	led3_Init();
	led4_Init();
	led5_Init();
	led6_Init();

	relay_Init();
}

// Aqui descre, ou chame, códigos que controlam tempos ou que sejam executados em tempos reguladores
void app_Tick(void) {
	// ATENÇÃO:  tempo de execução de TODOS os TICKS dentro dessa função NÃO pode ser superior a 1ms
	ledError_Tick();
	beep_Tick();
	gKey_Tick();
	relay_Tick();
	servo_Tick();

	// LEITURA DO TECLADO a cada 3ms
	static volatile uint timeReadKey = 0;
	static int bufKey[3];
	static uint idx = 0;
	if (timeReadKey) timeReadKey--;
	if (timeReadKey == 0) {
		timeReadKey = 3;

		// fazer debounce no teclado
		bufKey[idx%3] = (sw0_PIN&sw0_NUMPIN)?SW0:0;
		bufKey[idx%3] |= (sw1_PIN&sw1_NUMPIN)?SW1:0;
		bufKey[idx%3] |= (sw2_PIN&sw2_NUMPIN)?SW2:0;

		if ( (bufKey[0] == bufKey[1]) && (bufKey[1] == bufKey[2]) ) {
			control.key = bufKey[2]&0x7; // para que não pegar o botão de reconfigure
		}
		if (idx == 2) idx = 0; else idx++;
	}

	control.ticks++; // Contador de ticks do sistema
}


// Aqui descre, ou chame, códigos que não controlam tempos ou que não sejam executados em tempo reguladores, pelo menos diretamente
void app_Process(void) {
	static uint ledpos = 1;
	static uint ticks_d = 0;
	if ( (control.ticks%1000==0) && (control.ticks != ticks_d) ) {
		ticks_d = control.ticks;
		if (ledpos==0x01) led1_On(); else led1_Off();
		if (ledpos==0x02) led2_On(); else led2_Off();
		if (ledpos==0x04) led3_On(); else led3_Off();
		if (ledpos==0x08) led4_On(); else led4_Off();
		if (ledpos==0x10) led5_On(); else led5_Off();
		if (ledpos==0x20) led6_On(); else led6_Off();
		if (ledpos<0x20) ledpos<<=1; else ledpos=1;

		lcd_SetCursor(0, 0);
		lcd_printf("ticks: %d secs", control.ticks/1000);
		lcd_SetCursor(0, 1);
		//rprintf(STDIO_DEV1, "Distance: %dcm", us04_ReadMeasureCM(1));
	}
}
